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ABSTRACT 

A  method  (MATLAB  function)  to  define  continuous 
smooth  and  nonsmooth  paths  using  Hermite 
parametric  curves  is  presented.  The  defined  path 
can  be  used  as  a  reference  signal  in  a  driver 
model  for  feedback  control  of  vehicle  position. 
Four  input  formats  are  allowed  that  encompass 
two  and  three  dimensional  paths.  The 
formulation  can  account  for  path  crossover  and 
provides  a  method  to  deal  with  the  nonlinear 
relationship  relating  path  distance  and  path 
parameter  values. 

INTRODUCTION 

In  determining  ground  vehicle  dynamic 
performance  it  is  generally  accepted  that 
comparisons  between  simulated  and  test  results 
will  be  compared  for  precision  and  accuracy.  The 
test  environment  will  define  a  particular  course(s) 
that  will  be  used  to  define  the  excitation  to  the 
vehicle.  In  the  simulation  environment  the  same 
course(s)  must  be  replicated  to  excite  the 
dynamics  of  the  model.  Each  course  used  may, 
and  for  usefulness  will,  have  different 
characteristics  that  excite  the  dynamics  in 
different  ways  to  obtain  a  complete  spectrum  of 
inputs  to  the  system.  The  broader  the  spectrum 
the  better  characterization  of  the  system.  In  the 
simulation  environment  driver  models  are 
developed  to  ensure  that  the  vehicle  model  is 
traversing  the  desired  course(s)  in  the  same 
manner  as  in  the  test  environment  for  vehicle 
parameters  (speed,  steering,  etc.). 

A  path  must  be  defined  for  each  course  that 
represents  the  desired  vehicle  trajectory  and  will 
be  used  as  a  reference  signal  in  the  driver  model 
for  feedback  control  to  maintain  the  vehicle 


parameters  (speed,  steering,  etc.).  Ideally,  each 
position  on  the  path  should  be  uniquely 
identifiable. 

To  satisfy  these  desired  conditions  each  position 
on  the  path  is  defined  by  a  point.  A  parametric 
curve  is  then  defined  through  these  points  which 
will  allow  one  parameter  to  define  a  unique 
position  on  the  path.  This  formulation  also 
provides  for  path  crossover. 

The  number  of  points  required  and  the  method  of 
interpolation  between  them  to  accurately 
represent  the  trajectory  are  issues  that  depend 
on  the  resolution  required. 

PATH  DEFINITION 

Hermite  Cubic  (similar  to  Catmull-Rom  curves  but 
tangents  are  specified  and  not  inferred  from 
neighboring  points)  are  used  here  to  represent 
the  path  because  1)  they  can  be  parameterized, 
2)  they  have  the  desired  property  of  passing 
directly  through  each  point  interpolating 
between,  3)  they  can  be  made  to  be  continuous 
at  each  point,  and  4)  the  tangent  vectors  can  be 
explicitly  specified. 

Each  spatial  coordinate  is  specified  as  (y  and  z 
are  the  same) 

x{t)  =  at^  +bt^  +ct  +  d 

(1) 

a  third  order  polynomial  (cubic)  whose  derivative 
is 


1 


x{t)  =  3at^  +  2bt  +  c 


where  the  parameter 


(2) 


b  b 

s  =  (t)  +  y^(t)  +  z^{t)dt 


(9) 


^E[0,1]. 


The  solution  for  the  four  coefficients  requires  four 
equations.  Equations  (1)  and  (2)  are  evaluated 
at  both  limits  of  (3)  to  get  the  solution  for  the 
coefficients  in  (1)  as 
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Taking  the  inverse  (see  Appendix  A)  gives 
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The  coefficients  for  each  segment  (between 
points)  is  defined  by  equations  (1)  and  (3). 


where  (9)  is  over  all  path  segments.  The 
relationship  in  (9)  is  clearly  nonlinear.  A  value  of 
the  parameter  can  easily  be  related  to  the  path 
distance,  but  the  path  distance  cannot  easily  be 
related  to  the  parameter.  However,  since  the 
parameter  and  distance  along  the  path  are  both 
monotonically  increasing  values,  a  lookup  table 
can  be  generated  that  can  be  used  to  lookup  a 
value  of  the  parameter  for  a  particular  path 
distance.  With  this  solution  the  nonlinear 
relationship  between  path  distance  and 
parameter  can  easily  be  handled. 

IMPLEMENTATION 

Creation  of  a  path  requires  points  defining 
positions  on  the  path  and  tangent  vectors  at  each 
point  to  define  the  orientation  of  the  path  passing 
through  that  point. 

Consideration  is  also  given  to  the  ability  to  create 
smooth  or  non-smooth  ("sharp")  paths.  This  is 
accomplished  by  allowing  two  tangent  vectors  to 
be  specified  for  each  point.  The  first  is  used  if 
the  point  is  at  the  end  of  a  segment,  the  second 
if  at  the  beginning  of  a  segment.  This  allows 
"reorientation"  at  that  point  for  the  next  segment 
of  the  path. 


Since  the  parameter  gives  a  unique  position  on 
the  path  it  is  important  to  relate  this  to  the 
spatial  domain  in  terms  of  knowing  how  far  the 
vehicle  has  traveled  on  the  path  or  what  value  of 
the  parameter  is  needed  to  get  to  a  certain 
distance  on  the  path.  Any  point  on  a  segment  of 
the  path  is 

r  =  r{t)  =  (x(0,  y{t),  z(0),  a<t<b 

(6) 

where  the  coefficients  are  different  for  each 
segment,  but  the  limits  in  (6)  on  the  parameter 
may  be  the  same.  From  this  the  distance  is 
related  to  the  integral  of  the  path  speed 

(7) 

ds  =  \dr\  =  |r(^)|(if  =  vdt 


Consideration  is  also  given  for  three  dimensional 
as  well  as  two  dimensional  paths.  The  three 
dimensional  capability  will  allow  better  control  for 
traversing  a  path  with  different  elevations. 

In  the  MATLAB  implementation,  a  path  is 
"created"  by  calling  the  function 

makepath ( ' input . path ' ,  ' output . coef ' , 

' output . tbl ' ,  dp,  ds) ; 

where  "input. path"  is  the  path  definition  file 
name,  "output. coef"  is  the  coefficient  output  file 
name,  "output. tbl"  is  the  parameter  versus  path 
distance  file  name,  dp  is  the  parameter 
increment  used  to  generate  the  internal  p  and  s 
relationship,  and  ds  is  the  distance  increment 
used  to  generate  parameter  versus  path  distance 
file  "output. tbl"  for  equally  spaced  points  on  the 
path. 


(8) 
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PATH  FILE  FORMATS 

All  files,  input  and  output,  are  ASCII  text  files. 
The  path  input  file,  "input. path",  is  a  sequence 
of  points  and  tangent  vectors  defining  the  path. 
Any  line  that  starts  with  a  "%"  or  or  is  a 
comment.  There  are  four  possible  input  formats: 

2D 

Each  line  contains  three  values 
[x  y  theta] 

where  the  tangent  vector  is  assumed  to  be 

x'=cos(theta),  y'=sin(theta) 

Note  that  for  the  2D  format  the  magnitude  of  the 
tangent  vector  is  assumed  to  be  unity. 

2D  BRK 

Each  line  contains  four  values 

[x  y  theta  theta  1] 

where  the  tangent  vectors  are  assumed  to  be 
x'=cos(theta),  y'=sin(theta) 
if  at  the  end  of  a  segment  and 

x'=cos(thetal),  y'=sin(thetal) 
if  at  the  beginning  of  a  segment. 

3D 

Each  line  contains  six  values 

[x  y  z  x'  y'  z'] 

where  the  tangent  vector  is 

x'  y'  z' 

3D  BRK 

Each  line  contains  nine  values 

[X  y  z  X'  y'  z'  x"  y"  z"] 

where  the  tangent  vectors  are  assumed  to  be 

x'  y'  z' 

if  at  the  end  of  a  segment  and 
X"  y"  z" 


if  at  the  beginning  of  a  segment. 

The  "output. coef"  file  has  the  form  of 

[0;  ax;  bx;  cx;  dx;  ay;  by;  cy;  dy;  az;  bz;  cz;  dz] 

[1;  ax;  bx;  cx;  dx;  ay;  by;  cy;  dy;  az;  bz;  cz;  dz] 

[2;  ax;  bx;  cx;  dx;  ay;  by;  cy;  dy;  az;  bz;  cz;  dz] 


where  the  first  line  is  the  point  number  then 
three  sets  of  four  lines  for  x,  y,  and  z  coefficient 
values  respectively. 

The  "output. tbi"  file  has  the  form  of 

[P,  s] 

RESULTS 

The  following  will  illustrate  the  usage  for  smooth 
and  nonsmooth  paths. 

The  smooth  path  example  is  one  typical  of  a  lane 
change  for  a  vehicle.  The  path  is  described  as 
follows 

- BOF - 

%lane 

%3D  format  but  2D  curve 
0  0  0  5  0  0 
5  0  0  5  0  0 
10  5  0  5  1  0 
15  5  0  5  -1  0 
20  0  0  5  0  0 
25  0  0  5  0  0 
- EOF - 

See  Appendix  B  for  the  "output. coef"  and 
"output. tbi"  files.  Figure  1  shows  the  continuous 
path  generated  from  the  input  points  above. 


lane 


Figure  1  -  Smooth  Path 
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Figure  2  shows  the  nonlinear  relationship 
between  the  path  distance  and  parameter. 


lane 


Figure  2  -  Distance  versus  Parameter  (S) 

The  nonsmooth  path  example  is  a  three  sided 
square  and  line  segment  that  demonstrates  path 
crossover.  The  path  is  described  as  follows 

- BOF - 

%square  with  crossover 
%2D  format 
0  0  90 
0  5  90  0 
5  5  0  -90 
5  0  -90  135 
-10  10  135 
- EOF - 

See  Appendix  C  for  the  "output. coef"  and 
"output. tbi"  files.  Figure  3  shows  the  continuous 
path  generated  from  the  input  points  above  for 
the  nonsmooth  path. 


square 


-fi  -6  -4  -2  0  2  4 


Figure  3  -  Nonsmooth  Path 


Figure  4  shows  the  nonlinear  relationship 
between  the  path  distance  and  parameter  for  the 
nonsmooth  crossover  path. 


square 


Figure  4  -  Distance  versus  Parameter  (NS) 

CONCLUSION 

The  results  demonstrate  the  utility  of  Flermite 
parametric  curves  for  input  to  driver  models  and 
their  versatility  by  being  able  to  describe 
smooth/nonsmooth  and  multiple  crossover  paths 
with  ease. 

Due  to  the  monotonic  nature  of  the  path 
parameter  and  distance  a  lookup  table  can  be 
used  to  relate  the  two  path  parameters  as  shown 
in  Figures  2  and  4.  For  the  lane  and  square  path 
examples  the  end  distance  and  parameter  values 
are  29.7  and  4.98,  33.0  and  3.96  respectively 
(dp  =  0.01,  ds=  0.1).  These  values  correlate 
with  the  number  and  displacement  of  the  points 
in  the  respective  example  input  file. 

CONTACT 

The  author  is  a  an  engineer  at  the  U.S.  Army 
Tank-automotive  and  Armaments  Command, 
Research,  Development  and  Engineering  Center 
(TACOM-TARDEC).  Interested  parties  can 
contact  the  author  at  the  U.S.  Army  Tank- 
automotive  and  Armaments  Command,  ATTN: 
AMSTA-TR-N/MS157,  Warren,  Michigan  48397- 
5000,  bylsmaw(g)tacom. army. mil. 
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Research,  Development  and  Engineering  Center,  NAC  - 
National  Automotive  Center. 
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APPENDIX  A  -  Coefficient  Matrix  Inverse 


Each  point  is  numbered,  starting  from  zero,  so  the  parameter  varies  from  0  to  1,  starting  at  p^\.o where 
p^  of  the  next  point  will  equal  p^^^  of  the  previous  point.  For  each  interval  between  points  then 

x{p-p„)  =  a{p-p„y  +bip-p„f  +c{p-pj  +  d 


and 


x{p- Pn)  =  Mp- Pnf  +^b{p- p„)  +  c 
where  these  two  equations  are  evaluated  at 


gives 


Now 


xip 

x{p 

xip 

xip 


Pn) 

Pn) 

Pn) 

Pn) 


P  =  Pn 

P  =  Pn+\ 

P  =  Pn 

P  =  Pn+\ 


o 

o 

o 

a 

fill 

b 

0  0  10 

c 

_3  2  1  0_ 

_d_ 

=^Cofactor{A^ij) 

det^ 


where 


Cofactor{A.j)  =  (-1)'^"  Z).. 


and =subdeterminant  is  formed  by  deleting  the  ith  row  and  the  jth  column  of  and  taking  the 
determinant. 


det^  =  0(-l)‘^‘Z)ii  +0(-l)‘^"Z)i2  +0(-l)‘"^Z)i3  +1(-1) 


,1+4 


1  1  1 
0  0  1 
3  2  1 


,2+3 


1  1 
3  21 


=  (-l)(-l)(2-3)  =  -l 
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1  1  1 

C„=(-1)'"'0  1  0=(-l)'"'(l)(-l)^^^^  ^=(l)(l)(l)(0-2)  =  -2 
2  1  0 

1  1  1 

Q2=(-1)'"^0  1  0=(-l)^"^(l)(-iy^^°  J=(-l)(l)(l)(0-3)  =  3 
3  1  0 

1  1  1 

Q3=(-1)^^^  0  0  0  =(-ir(l)(-l)'^^°  °=(1)(1)(1)(0)  =  0 
3  2  0 

1  1  1 

q4=(-l)'^^0  0  1=(-1)'^^(1)(-1)^^^^  ^=(-l)(l)(-l)(2-3)  =  -l 
3  2  1 

0  0  1 

Q,  =(-l)^^'0  1  0=(-l)^^'(l)(-l)^^^°  J=(-l)(l)(l)(0-2)  =  2 
2  1  0 

0  0  1 

Q,=(-1)^"^0  1  0=(-l)^"^(l)(-iy^^°  J=(l)(l)(l)(0-3)  =  -3 
3  1  0 

0  0  1 

Q3=(-1)^^^0  0  0  =(-l)^^^(l)(-iy^^°  °=(-l)(l)(l)(0-0)  =  0 
3  2  0 

0  0  0 

Q,  =(-l)"^'0  0  1  =  (-1)2^\0)  =  0 
3  2  1 

0  0  1 

C3,  =  (-1)^^'  111=  (-1)^^'  (1)(-1)^^^  ^  J  =  (1)(1)(1)(1  -  2)  =  -1 
2  1  0 

0  0  1 

C33  =  (-1)^^^  111=  (-1)^-2 1  1  ^  _3)  =  2 

3  1  0 
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0  0  1 

C33=(-1)^"M  1  1  ^=(l)(l)(l)(2-3)  =  -l 

3  2  0 

0  0  0 

C3^=(-1)3+M  1  1  =(-l)'^^(0)  =  0 

3  2  1 

0  0  1 

Q,  =(-!/"' 1  1  1  =(-ir^(l)(-iy^^^  J=(-l)(l)(l)(l-0)  =  -l 

0  1  0 

0  0  1 

Q3=(-irM  1  1  =(-1)^-2  J=(l)(l)(l)(l-0)  =  1 

0  1  0 

0  0  1 

C43=(-l)"^'l  1  1  =(-l)^^'(0)  =  0 

0  0  0 

0  0  0 

C44=(-l)'"'l  1  1  =(-l)""'(0)  =  0 

0  0  1 

■-2  3  0  -1]^  [2-21  r 

,  1  2  -3  0  0  -3  3  -2  -1 

\A~^^  =—  = 

^-1-12-10  0010 

-1  1  0  oj  [l  0  0  0_ 
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APPENDIX  B  -  Lane  Path  Example  Output  Files 


- BOF "output . coef " 

0.000000 

0.000000 

0.000000 

5.000000 

0.000000 

0.000000 

0.000000 

0.000000 

0.000000 

0.000000 

0.000000 

0.000000 

0.000000 

1.000000 

0.000000 

0.000000 

5.000000 

5.000000 

-9.000000 

14.000000 

0.000000 

0.000000 

0.000000 

0.000000 

0.000000 

0.000000 

2.000000 

0.000000 

0.000000 

5.000000 

10.000000 

0.000000 

-1.000000 

1.000000 

5.000000 

0.000000 

0.000000 

0.000000 

0.000000 

3.000000 

0.000000 

0.000000 

5.000000 

15.000000 

9.000000 

-13.000000 

-1.000000 

5.000000 

0.000000 

0.000000 

0.000000 

0.000000 

4.000000 

0.000000 

0.000000 

5.000000 

20.000000 

0.000000 

0.000000 

0.000000 

0.000000 

0.000000 

0.000000 

0.000000 

0.000000 

- EOF - "output . coef" 


- BOF - "output . tbl 

0.000000  0.000000 
0.100000  0.010000 
0.200000  0.030000 
0.300000  0.050000 
0.400000  0.070000 
0.500000  0.090000 
0.600000  0.110000 
0.700000  0.130000 
0.800000  0.150000 
0.900000  0.170000 
1.000000  0.190000 

1.100000  0.210000 

1.200000  0.230000 

1.300000  0.250000 

1.400000  0.270000 

1.500000  0.290000 

1.600000  0.310000 

1.700000  0.330000 

1.800000  0.350000 

1.900000  0.370000 
2.000000  0.390000 

2.100000  0.410000 

2.200000  0.430000 

2.300000  0.450000 

2.400000  0.470000 

2.500000  0.490000 

2.600000  0.510000 

2.700000  0.530000 

2.800000  0.550000 

2.900000  0.570000 
3.000000  0.590000 

3.100000  0.610000 

3.200000  0.630000 

3.300000  0.650000 

3.400000  0.670000 

3.500000  0.690000 

3.600000  0.710000 

3.700000  0.730000 

3.800000  0.750000 

3.900000  0.770000 
4.000000  0.790000 

4.100000  0.810000 

4.200000  0.830000 

4.300000  0.850000 

4.400000  0.870000 

4.500000  0.890000 

4.600000  0.910000 

4.700000  0.930000 

4.800000  0.950000 

4.900000  0.970000 
5.000000  0.990000 

5.100000  1.000000 

5.200000  1.019925 

5.300000  1.039573 

5.400000  1.058784 

5.500000  1.077465 

5.600000  1.095575 

5.700000  1.113117 

5.800000  1.130114 

5.900000  1.146542 
6.000000  1.162500 

6.100000  1.178001 

6.200000  1.193079 

6.300000  1.207782 

6.400000  1.222136 

6.500000  1.236166 

6.600000  1.249930 

6.700000  1.263398 

6.800000  1.276642 

6.900000  1.289679 
7.000000  1.302496 

7.100000  1.315137 


7.200000  1.327620 

7.300000  1.339957 

7.400000  1.352142 

7.500000  1.364208 

7.600000  1.376166 

7.700000  1.388025 

7.800000  1.399795 

7.900000  1.411478 
8.000000  1.423091 

8.100000  1.434644 

8.200000  1.446144 

8.300000  1.457600 

8.400000  1.469019 

8.500000  1.480408 

8.600000  1.491774 

8.700000  1.503126 

8.800000  1.514473 

8.900000  1.525819 
9.000000  1.537174 

9.100000  1.548543 

9.200000  1.559935 

9.300000  1.571360 

9.400000  1.582823 

9.500000  1.594332 

9.600000  1.605896 

9.700000  1.617521 

9.800000  1.629216 

9.900000  1.640998 
10.000000  1.652874 

10.100000  1.664851 

10.200000  1.676938 

10.300000  1.689147 

10.400000  1.701502 

10.500000  1.714014 

10.600000  1.726687 

10.700000  1.739535 

10.800000  1.752605 

10.900000  1.765891 
11.000000  1.779404 

11.100000  1.793209 

11.200000  1.807295 

11.300000  1.821699 

11.400000  1.836469 

11.500000  1.851610 

11.600000  1.867189 

11.700000  1.883225 

11.800000  1.899742 

11.900000  1.916831 
12.000000  1.934468 

12.100000  1.952676 

12.200000  1.971451 

12.300000  1.990744 

12.400000  2.000355 

12.500000  2.019990 

12.600000  2.039652 

12.700000  2.059342 

12.800000  2.079058 

12.900000  2.098799 
13.000000  2.118565 

13.100000  2.138353 

13.200000  2.158164 

13.300000  2.177995 

13.400000  2.197845 

13.500000  2.217715 

13.600000  2.237601 

13.700000  2.257504 

13.800000  2.277422 

13.900000  2.297353 
14.000000  2.317297 

14.100000  2.337252 

14.200000  2.357217 

14.300000  2.377191 

14.400000  2.397172 


14.500000  2.417160 

14.600000  2.437152 

14.700000  2.457148 

14.800000  2.477147 

14.900000  2.497147 
15.000000  2.517147 

15.100000  2.537145 

15.200000  2.557141 

15.300000  2.577132 

15.400000  2.597119 

15.500000  2.617098 

15.600000  2.637070 

15.700000  2.657033 

15.800000  2.676986 

15.900000  2.696927 
16.000000  2.716856 

16.100000  2.736770 

16.200000  2.756670 

16.300000  2.776553 

16.400000  2.796418 

16.500000  2.816265 

16.600000  2.836091 

16.700000  2.855897 

16.800000  2.875680 

16.900000  2.895441 
17.000000  2.915177 

17.100000  2.934887 

17.200000  2.954571 

17.300000  2.974227 

17.400000  2.993855 

17.500000  3.003429 

17.600000  3.022622 

17.700000  3.041290 

17.800000  3.059384 

17.900000  3.076882 
18.000000  3.093832 

18.100000  3.110276 

18.200000  3.126189 

18.300000  3.141677 

18.400000  3.156733 

18.500000  3.171425 

18.600000  3.185752 

18.700000  3.199791 

18.800000  3.213510 

18.900000  3.226983 
19.000000  3.240228 

19.100000  3.253232 

19.200000  3.266048 

19.300000  3.278691 

19.400000  3.291164 

19.500000  3.303480 

19.600000  3.315666 

19.700000  3.327732 

19.800000  3.339690 

19.900000  3.351538 
20.000000  3.363298 

20.100000  3.374979 

20.200000  3.386592 

20.300000  3.398145 

20.400000  3.409644 

20.500000  3.421096 

20.600000  3.432511 

20.700000  3.443898 

20.800000  3.455265 

20.900000  3.466617 
21.000000  3.477963 

21.100000  3.489310 

21.200000  3.500665 

21.300000  3.512036 

21.400000  3.523431 

21.500000  3.534857 

21.600000  3.546321 

21.700000  3.557830 
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21.800000  3.569394 

21.900000  3.581024 
22.000000  3.592729 

22.100000  3.604516 

22.200000  3.616392 

22.300000  3.628369 

22.400000  3.640459 

22.500000  3.652687 

22.600000  3.665049 

22.700000  3.677560 

22.800000  3.690233 

22.900000  3.703111 
23.000000  3.716183 

23.100000  3.729464 

23.200000  3.743013 

23.300000  3.756821 

23.400000  3.770911 

23.500000  3.785350 

23.600000  3.800109 

23.700000  3.815298 

23.800000  3.830876 


23.900000  3.846946 
24.000000  3.863506 

24.100000  3.880586 

24.200000  3.898256 

24.300000  3.916503 

24.400000  3.935309 

24.500000  3.954625 

24.600000  3.974343 

24.700000  3.994294 

24.800000  4.004294 

24.900000  4.024294 
25.000000  4.044294 

25.100000  4.064294 

25.200000  4.084294 

25.300000  4.104294 

25.400000  4.124294 

25.500000  4.144294 

25.600000  4.164294 

25.700000  4.184294 

25.800000  4.204294 

25.900000  4.224294 


26.000000  4.244294 

26.100000  4.264294 

26.200000  4.284294 

26.300000  4.304294 

26.400000  4.324294 

26.500000  4.344294 

26.600000  4.364294 

26.700000  4.384294 

26.800000  4.404294 

26.900000  4.424294 
27.000000  4.444294 

27.100000  4.464294 

27.200000  4.484294 

27.300000  4.504294 

27.400000  4.524294 

27.500000  4.544294 

27.600000  4.564294 

27.700000  4.584294 

27.800000  4.604294 

27.900000  4.624294 
28.000000  4.644294 


28.100000  4.664294 

28.200000  4.684294 

28.300000  4.704294 

28.400000  4.724294 

28.500000  4.744294 

28.600000  4.764294 

28.700000  4.784294 

28.800000  4.804294 

28.900000  4.824294 
29.000000  4.844294 

29.100000  4.864294 

29.200000  4.884294 

29.300000  4.904294 

29.400000  4.924294 

29.500000  4.944294 

29.600000  4.964294 

29.700000  4.984294 
- EOF " output . tbl 
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APPENDIX  C  -  Square  Path  Example  Output  Files 


- BOF - " 

0.000000 
0.000000 
0.000000 
0.000000 
0.000000 
-8.000000 
12.000000 
1.000000 
0.000000 
0.000000 
0.000000 
0.000000 
0.000000 
1.000000 
-8.000000 
12.000000 
1.000000 
0.000000 
0.000000 
0.000000 
0.000000 
5.000000 
0.000000 
0.000000 
0.000000 
0.000000 
2.000000 
0.000000 
0.000000 
0.000000 
5.000000 
8.000000 
-12.000000 
-1.000000 
5.000000 
0.000000 
0.000000 
0.000000 
0.000000 
3.000000 
28.585786 
-42 . 878680 
-0 . 707107 
5.000000 
-18.585786 
27 . 878680 
0.707107 
0.000000 
0.000000 
0.000000 
0.000000 
0.000000 
- EOF - " 


output . coef " 


output . coef" 


- BOF - " output . tbl " 

7.300000  1.464337 

0.000000  0.000000 

7.400000  1.478652 

0.100000  0.052260 

7.500000  1.492943 

0.200000  0.089609 

7.600000  1.507231 

0.300000  0.119547 

7.700000  1.521536 

0.400000  0.145507 

7.800000  1.535880 

0.500000  0.169006 

7.900000  1.550278 

0.600000  0.190678 

8.000000  1.564763 

0.700000  0.210989 

8.100000  1.579342 

0.800000  0.230256 

8.200000  1.594059 

0.900000  0.248655 

8.300000  1.608919 

1.000000  0.266356 

8.400000  1.623973 

1.100000  0.283490 

8.500000  1.639228 

1.200000  0.300150 

8.600000  1.654754 

1.300000  0.316362 

8.700000  1.670553 

1.400000  0.332239 

8.800000  1.686720 

1.500000  0.347808 

8.900000  1.703279 

1.600000  0.363115 

9.000000  1.720293 

1.700000  0.378205 

9.100000  1.737899 

1.800000  0.393101 

9.200000  1.756174 

1.900000  0.407841 

9.300000  1.775259 

2.000000  0.422447 

9.400000  1.795355 

2.100000  0.436945 

9.500000  1.816733 

2.200000  0.451362 

9.600000  1.839773 

2.300000  0.465712 

9.700000  1.865262 

2.400000  0.480025 

9.800000  1.894291 

2.500000  0.494314 

9.900000  1.929616 

2.600000  0.508603 

10.000000  1.982566 

2.700000  0.522912 

10.100000  2.043514 

2.800000  0.537259 

10.200000  2.083135 

2.900000  0.551666 

10.300000  2.114115 

3.000000  0.566158 

10.400000  2.140778 

3.100000  0.580749 

10.500000  2.164629 

3.200000  0.595479 

10.600000  2.186608 

3.300000  0.610352 

10.700000  2.207162 

3.400000  0.625429 

10.800000  2.226605 

3.500000  0.640706 

10.900000  2.245164 

3.600000  0.656258 

11.000000  2.263006 

3.700000  0.672096 

11.100000  2.280259 

3.800000  0.688285 

11.200000  2.296974 

3.900000  0.704894 

11.300000  2.313276 

4.000000  0.721968 

11.400000  2.329222 

4.100000  0.739608 

11.500000  2.344836 

4.200000  0.757959 

11.600000  2.360204 

4.300000  0.777135 

11.700000  2.375320 

4.400000  0.797339 

11.800000  2.390260 

4.500000  0.818846 

11.900000  2.405019 

4.600000  0.842128 

12.000000  2.419655 

4.700000  0.867846 

12.100000  2.434168 

4.800000  0.897329 

12.200000  2.448599 

4.900000  0.933661 

12.300000  2.462961 

5.000000  0.990400 

12.400000  2.477279 

5.100000  1.048000 

12.500000  2.491571 

5.200000  1.086372 

12.600000  2.505859 

5.300000  1.116831 

12.700000  2.520160 

5.400000  1.143143 

12.800000  2.534501 

5.500000  1.166817 

12.900000  2.548893 

5.600000  1.188653 

13.000000  2.563369 

5.700000  1.209089 

13.100000  2.577940 

5.800000  1.228434 

13.200000  2.592639 

5.900000  1.246909 

13.300000  2.607488 

6.000000  1.264681 

13.400000  2.622517 

6.100000  1.281874 

13.500000  2.637758 

6.200000  1.298563 

13.600000  2.653251 

6.300000  1.314819 

13.700000  2.669023 

6.400000  1.330735 

13.800000  2.685155 

6.500000  1.346322 

13.900000  2.701663 

6.600000  1.361660 

14.000000  2.718643 

6.700000  1.376762 

14.100000  2.736191 

6.800000  1.391680 

14.200000  2.754389 

6.900000  1.406430 

14.300000  2.773384 

7.000000  1.421052 

14.400000  2.793372 

7.100000  1.435557 

14.500000  2.814620 

7.200000  1.449983 

14.600000  2.837501 

14 . 700000 

14.800000 

14.900000 
15.000000 

15.100000 

15.200000 

15.300000 

15.400000 

15.500000 

15.600000 

15.700000 

15.800000 

15.900000 
16.000000 

16.100000 

16.200000 

16.300000 

16.400000 

16.500000 

16.600000 

16.700000 

16.800000 

16.900000 
17.000000 

17 . 100000 

17.200000 

17.300000 

17 . 400000 

17.500000 

17.600000 

17 . 700000 

17.800000 

17.900000 
18.000000 

18.100000 

18.200000 

18.300000 

18.400000 

18.500000 

18.600000 

18.700000 

18.800000 

18.900000 
19.000000 

19.100000 

19.200000 

19.300000 

19.400000 

19.500000 

19.600000 

19.700000 

19.800000 

19.900000 
20.000000 

20.100000 

20.200000 

20.300000 

20.400000 

20.500000 

20.600000 

20.700000 

20.800000 

20.900000 
21.000000 
21.100000 
21.200000 

21.300000 

21 . 400000 

21.500000 
21.600000 

21 . 700000 
21.800000 

21.900000 
22.000000 


2 .862677 
2 .891253 
2.925869 
2.975631 
3.022848 
3.043864 
3.059505 
3.072380 
3.083757 
3.094082 
3.103613 
3.112527 
3.120953 
3.128921 
3.136503 
3.143816 
3.150903 
3.157693 
3.164283 
3.170740 
3.176959 
3.183057 
3.189037 
3.194837 
3.200581 
3.206153 
3.211670 
3.217066 
3.222389 
3.227626 
3.232784 
3.237876 
3.242892 
3.247853 
3.252745 
3.257585 
3.262371 
3.267101 
3.271792 
3.276421 
3.281030 
3.285567 
3.290102 
3.294554 
3.299007 
3.303399 
3.307773 
3.312113 
3.316416 
3.320709 
3.324947 
3.329185 
3.333375 
3.337553 
3.341709 
3.345833 
3.349956 
3.354031 
3.358105 
3.362154 
3.366183 
3.370209 
3.374197 
3.378184 
3.382151 
3.386102 
3.390052 
3.393969 
3.397886 
3.401790 
3.405677 
3.409565 
3.413429 
3.417291 
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22.100000  3.421145 

22.200000  3.424984 

22.300000  3.428823 

22.400000  3.432648 

22.500000  3.436467 

22.600000  3.440285 

22.700000  3.444088 

22.800000  3.447891 

22.900000  3.451688 
23.000000  3.455478 

23.100000  3.459267 

23.200000  3.463049 

23.300000  3.466828 

23.400000  3.470606 

23.500000  3.474378 

23.600000  3.478150 

23.700000  3.481920 

23.800000  3.485688 

23.900000  3.489456 
24.000000  3.493222 

24.100000  3.496988 

24.200000  3.500755 

24.300000  3.504523 

24.400000  3.508290 

24.500000  3.512060 

24.600000  3.515832 

24.700000  3.519604 

24.800000  3.523383 


24.900000  3.527162 
25.000000  3.530944 

25.100000  3.534734 

25.200000  3.538524 

25.300000  3.542321 

25.400000  3.546124 

25.500000  3.549927 

25.600000  3.553746 

25.700000  3.557565 

25.800000  3.561392 

25.900000  3.565230 
26.000000  3.569069 

26.100000  3.572925 

26.200000  3.576786 

26.300000  3.580652 

26.400000  3.584540 

26.500000  3.588427 

26.600000  3.592333 

26.700000  3.596250 

26.800000  3.600168 

26.900000  3.604119 
27.000000  3.608069 

27.100000  3.612039 

27.200000  3.616026 

27.300000  3.620014 

27.400000  3.624042 

27.500000  3.628071 

27.600000  3.632123 


27.700000  3.636197 

27.800000  3.640274 

27.900000  3.644398 
28.000000  3.648521 

28.100000  3.652680 

28.200000  3.656858 

28.300000  3.661051 

28.400000  3.665290 

28.500000  3.669528 

28.600000  3.673824 

28.700000  3.678127 

28.800000  3.682471 

28.900000  3.686846 
29.000000  3.691242 

29.100000  3.695694 

29.200000  3.700149 

29.300000  3.704686 

29.400000  3.709223 

29.500000  3.713837 

29.600000  3.718466 

29.700000  3.723163 

29.800000  3.727894 

29.900000  3.732685 
30.000000  3.737525 

30.100000  3.742424 

30.200000  3.747385 

30.300000  3.752408 

30.400000  3.757500 


30.500000  3.762666 

30.600000  3.767903 

30.700000  3.773236 

30.800000  3.778632 

30.900000  3.784158 
31.000000  3.789730 

31.100000  3.795485 

31.200000  3.801297 

31.300000  3.807277 

31.400000  3.813388 

31.500000  3.819608 

31.600000  3.826079 

31.700000  3.832687 

31.800000  3.839477 

31.900000  3.846582 
32.000000  3.853917 

32.100000  3.861523 

32.200000  3.869492 

32.300000  3.877947 

32.400000  3.886895 

32.500000  3.896466 

32.600000  3.906839 

32.700000  3.918276 

32.800000  3.931322 

32.900000  3.947092 
33.000000  3.968562 
- EOF " output . tbl 
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APPENDIX  D  -  Makepath  Listing 


%  Make  Path  (makepath. m) 

%  Usage: 

%  makepath  [input]  [output . coef]  [output. tbl]  [dp]  [ds] 

%  Example:  makepath (' input . path ' ,  ' output . coef ' ,  ' output . tbl ' ,  0.01,  0.1); 

%  where  input  format  is 

%  2D  =  [x  y  theta]  where  x ' =cos (theta) ,  y ' =sin (theta) 

%  2D  BRK  =  [x  y  theta  thetal] 

%  3D  =  [xyzx'  y'  z'] 

%  3D  BRK  =  [xyzx'  y'  z'  x'  y'  z'] 

%  for  each  point. 

%  If  2nd  set  of  tangents  is  encountered  it  will  restart  at  the  same 
%  point  with  the  new  tangent  vector. 

%  Comment  lines  start  with  '%'  or  '#'  or  '*' 

%  dp  =  parameter  increment,  ds  =  distance  increment 

function  [A] =makepath (in, out, tbl, dp, ds) ; 

LINE_LEN=  1024;  %  line  length 
MAX_PTS=  1000;  %  max  number  of  points 
PI=  4 . 0*atan (1 . 0) ; 

DEG2RAD=  PI /ISO. 0; 

FLG_2D  =3; 

FLG_2D_BRK=  4; 

FLG_3D=  6; 

FLG_3D_BRK=  9; 

mh  =  [2.0,  -2.0,  1.0,  1.0;  -3.0,  3.0,  -2.0,  -1.0;  0.0,  0.0,  1.0,  0.0;  1.0,  0.0,  0.0,  0.0];  %  transform  matrix 

fprintf ( ' \n\n\nMake  Path\n\n'); 

% - open  files  and  process 

fin  =  fopen (in, ' rt ' ) ; 
font  =  fopen (out,  'w' )  ; 
foutl  =  fopen (tbl, 'w' ) ; 

fprintf ( ' Processing  %s  . . . \n ' , in) ; 

num__pts  =  1; 
val  =  0; 

line  =  fgetl(fin); 

while  (lined)  ==  '%'  I  line(l)  ==  '#'  I  line(l)  ==  '*'), 
line  =  fgetl(fin); 
end 

while  (line  ~=  -1), 

fprintf (  '  [ %s ] \n ' , line ) ; 

val  =  sscanf ( line , ' %f  %f  %f  %f  %f  %f  %f  %f  %f');  %  &x,  &y,  &z,  &xp,  &yp,  &zp,  &xpn,  &ypn,  &zpn) ; 

Ival  =  length (val); 

val  =  [val ', [zeros (9-length (val) , 1) ']]' ; 

X  =  val ( 1 ) ; 
y  =  val  (2 )  ; 
z  =  val  (3)  ; 
xp  =  val (4) ; 
yp  =  val  (5)  ; 
zp  =  val  (6)  ; 
xpn  =  val ( 7 ) ; 
ypn  =  val (8) ; 
zpn  =  val  (9) ; 
if  (val  ==  -1) 
break; 
end 

switch  (ival) 
case  (FLG_2D} 

% - 2D  =  [x  y  theta]  where  x ' =cos (theta) ,  y ' =sin (theta) 

pts . fig (num_pts)  =  FLG_2D; 
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pts . X (num_pts)  =  x; 
pts . y (num_pts)  =  y; 
pts . z (num_pts)  =  0.0; 
pts . zp (num_pts)  =  z; 

pts . xp (num_pts)  =  cos(DEG2RAD*pts.zp(num_pts)); 
pts  .  yp  (num__pts)  =  sin  (DEG2RAD*pts  .  zp  (num_pts)  )  ; 
pts . zp (num_pts)  =  0.0; 

pts . xpn (num_pts)  =  0.0; 
pts . ypn (num_pts)  =  0.0; 
pts . zpn (num_pts)  =  0.0; 

fprintf ( ' --%d-%f  %f  %f  %f  %f  %f  %f  %f  %f\n',  pts . fig (num_pts) , . . . 

pts . X (num_pts) ,  pts . y (num_pts ) ,  pts . z (num_pts ) , . . . 
pts . xp (num_pts) ,  pts . yp (num_pts ) ,  pts . zp (num_pts ) , . . . 
pts . xpn (num_pts) ,  pts . ypn (num_pts ) ,  pts . zpn (num_pts ) ) ; 


case  {FLG_2D_BRK} 

% - 2D  BRK  =  [x  y  theta  thetal] 

pts . fig (num_pts)  =  FLG_2D_BRK; 

pts . X (num_pts)  =  x; 

pts . y (num_pts)  =  y; 

pts . z (num_pts)  =  0.0; 

pts . zp (num_pts)  =  z; 

pts . zpn (num_pts)  =  xp; 

pts . xp (num_pts)  =  cos(DEG2RAD*pts.zp(num_pts)); 
pts . yp (num_pts)  =  sin (DEG2RAD*pts . zp (num_pts) ) ; 
pts . zp (num_pts)  =  0.0; 

pts . xpn (num_pts )  =  cos (DEG2RAD*pts . zpn (num_pts) ) ; 
pts  .  ypn  (num_pts)  =  sin  (DEG2RAD*pts  .  zpn  (num__pts)  )  ; 
pts . zpn (num_pts)  =  0.0; 

fprintf (' --%d-%f  %f  %f  %f  %f  %f  %f  %f  %f\n',  pts . fig (num_pts)  ,  . . . 

pts . X (num_pts) ,  pts . y (num_pts ) ,  pts . z (num_pts ) , . . . 
pts . xp (num_pts) ,  pts . yp (num_pts ) ,  pts . zp (num_pts ) , . . . 
pts . xpn (num_pts) ,  pts . ypn (num_pts ) ,  pts . zpn (num_pts ) ) ; 


case  {FLG_3D} 

% - 3D  =  [x  y  z  x'  y'  z'] 

pts . fig (num_pts)  =  FLG_3D; 
pts . X (num_pts)  =  x; 
pts . y (num_pts)  =  y; 
pts . z (num_pts)  =  z; 
pts . xp (num_pts)  =  xp; 
pts . yp (num_pts)  =  yp; 
pts . zp (num_pts)  =  zp; 
pts . xpn (num_pts)  =  0.0; 
pts . ypn (num_pts)  =  0.0; 
pts . zpn (num_pts)  =  0.0; 

case  {FLG_3D_BRK} 

% - 3D  BRK  =  [xyzx'  y'  z'  x'  y'  z'] 

pts . fig (num_pts)  =  FLG_3D_BRK; 

pts . X (num_pts)  =  x; 

pts . y (num_pts)  =  y; 

pts . z (num_pts)  =  z; 

pts . xp (num_pts)  =  xp; 

pts . yp (num_pts)  =  yp; 

pts . zp (num_pts)  =  zp; 

pts . xpn (num_pts )  =  xpn; 

pts . ypn (num_pts)  =  ypn; 

pts . zpn (num_pts)  =  zpn; 

otherwise 

fprintf (' Error :  Undefined  number  of  values  for  point  [%d]\n',k); 
exit (1)  ; 

end 

fprintf ( font %d-%f  %f  %f  %f  %f  %f  %f  %f  %f\n',  pts . fig (num_pts) , . . . . 

pts . X (num_pts) ,  pts . y (num_pts ) ,  pts . z (num_pts ) , . . . 
pts . xp (num_pts) ,  pts . yp (num_pts ) ,  pts . zp (num_pts ) , . . . . 
pts . xpn (num_pts) ,  pts . ypn (num_pts ) ,  pts . zpn (num_pts ) ) ; 

num_pts  =  num__pts  +  1; 
if  (num_pts  >=  MAX_PTS) 

fprintf (' Error :  MAX_PTS=%d  exceededXn ' , MAX_PTS ) ; 

stop; 

end 
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line  =  fgetl(fin); 

while  (lined)  ==  '%'  I  line(l)  ==  '#'  I  line(l)  == 
line  =  fgetl(fin); 
end 
end 

fclose (fin) ; 

fprintf ( ' - Read  in  %d  pointsXn ' , num_pts ) ; 

fprintf (' Processing  %s  ...\n',out); 

% - create  coefficients 

for  i  =  1  :  num_pts  -  2, 

% - set  vector  to  get  coefficients  between  points 

% - X 

inv. a  =  pts . x  (i) ; 
inv.b  =  pts.x(i+l); 

if  ((pts.flg(i)  ==  FLG_2D_BRK)  |  (pts.flg(i)  ==  FLG_3D_BRK) ) 
inv.c  =  pts.xpn(i); 
else 

inv.c  =  pts.xp(i); 
end 

inv.d  =  pts.xp(i+l); 

cx(i,:)  =  [mh* [inv. a; inv.b; inv. c; inv. d] ] ' ; 

% - y 

inv. a  =  pts . y  (i) ; 
inv.b  =  pts.y(i+l); 

if  ((pts. fig (i)  ==  FLG_2D_BRK)  |  (pts. fig (i)  ==  FLG_3D_BRK) ) 
inv.c  =  pts.ypn(i); 
else 

inv.c  =  pts.yp(i); 
end 

inv.d  =  pts.yp(i+l); 

cy(i,:)  =  [mh* [inv. a; inv.b; inv. c; inv. d] ] ' ; 

% - z 

inv. a  =  pts . z  (i) ; 
inv.b  =  pts.z(i+l); 

if  ((pts. fig (i)  ==  FLG_2D_BRK)  |  (pts. fig (i)  ==  FLG_3D_BRK) ) 
inv.c  =  pts.zpn(i); 
else 

inv. c  =  pts . zp (i) ; 
end 

inv.d  =  pts.zp(i+l); 

cz (i,  : )  =  [mh* [inv. a; inv.b; inv. c; inv. d] ]  ' ; 

% -  print  out  coefficients 

fprintf  ( f  out  ,'%f\adi“l); 

fprintf  ( font ,  '%f\n%f\n%f\n%f\n'  ,cx(i,l)  ,  cx{l,  2)  ,cx(i,3)  ,cx(i,4)  )  ; 
fprintf (font, '%f\n%f\n%f\n%f\n' ,cy(i,l) ,cy(i,2) ,cy(i,3) ,cy(i,4) ) ; 
fprintf ( font ,  '%f\n%f\n%f\n%f\n' ,cz (i,l) jCz (i,2) i cz  (i,3) ,cz (i,4) )  ; 

end 

fclose (font)  ; 

% - create  p,s  table  at  same  time 

sump (1, 1) =0 . 0;  %  start  at  0.0 
sump (1, 2) =0 . 0; 
cnt  =  2 ; 

for  i  =  1  :  num_pts  -  2, 
for  j  =  0  :  dp  :  1, 

ptx(cnt)  =  [j''3  j''2  j  l]*cx(i,:)'; 
ptvx(cnt)  =  [3*j''2  2*j  1  0]*cx(i,:)'; 
pty(cnt)  =  [j''3  j''2  j  l]*cy(i,:)'; 
ptvy(cnt)  =  [3*j''2  2*j  1  0]*cy(i,:)'; 
ptz  (cnt)  =  [j''3  j''2  j  l]*cz(i,:)'; 
ptvz(cnt)  =  [3*j-'2  2*j  1  0]*cz(i,:)'; 

sump (cnt, 1)  =  (i-l)+j; 

sump  (cnt,  2)  =  sqrt  (  ptvx(cnt)''2  +  ptvy(cnt)''2  +  ptvz(cnt)''2  )  *  dp; 
cnt  =  cnt  +  1; 
end 
end 
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% - make  plots 

figure  (1)  ; 
clf; 
cnt=l ; 

for  j  =  0  :  ds  :  sum  ( sump  ( :  ,  2 )  )  , 

p  =  interpl (cumsum ( sump ( : , 2 ) ) , sump ( : , 1 ) , j , ' linear ' ) ; 
fprintf ( f outl , ' %f  %f\n',j,p); 

1  =  floor  (p)  +  1 ; 
p  =  mod  (p,  1 )  ; 

ptx  (cnt)  =  [p''3  p''2  p  1]  *cx  (1,  :  )  '  ; 
ptvx(cnt)  =  [3*p''2  2*p  1  0]*cx(i,:)'; 
pty  (cnt)  =  [p''3  p''2  p  1]  *cy  (i,  :  )  '  ; 
ptvy(cnt)  =  [3*p''2  2*p  1  0]*cy(i,:)'; 
ptz  (cnt)  =  [p''3  p''2  p  1]  *cz  (1,  :  )  '  ; 
ptvz  (cnt)  =  [3*p''2  2*p  1  0]*cz(i,:)'; 
cnt  =  cnt  +  1 ; 
end 

fclose (foutl)  ; 

h  =  plot (ptx (1 : cnt-1 ) , pty (1 : cnt-1 ) ,  ' r '  )  ; 

%  print ( ' -djpeg99 ' , [char (gg (1) )  ' . jpg' ] ) 

%  print  -dps2  -Past4000tn  %see  help  print 

set (gca, ' Font Size ' , 12 ) ; 

set (gca, ' FontWeight ' , 'bold ' ) ; 

set (h, ' LineWidth ' , 3) ; 

axis  equal 

figure (2 ) ; 
clf; 

h  =  plot (cumsum (sump (:, 2 )), sump (:, 1 ) ) 

set (gca, ' Font Size ' , 12 ) 

set (gca, ' FontWeight ' , 'bold ' ) ; 

set (h, ' LineWidth ' , 3) ; 

xlabel ( ' distance ' ) ; 

ylabel ( 'p ' ) ; 

figure  (3)  ; 
clf  ; 

h=plot ( sump ( : , 1 ) , sump ( : , 2 ) ) 
set (gca,  ' FontSize '  ,  12 ) 
set (gca, ' FontWeight ' , 'bold ' ) ; 
set (h, ' LineWidth ' , 3) ; 
xlabel  (  'p ' ) ; 
ylabel ( ' vel '  ) ; 

%end 
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